#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include <mc_map/map_base.h>
#include <mc_path_planner/graph_planner/a_star.h>
#include <mc_path_planner/graph_planner/jps.h>

class TestPathGen{
    public:
        TestPathGen();

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);

    private:
        ros::NodeHandle nh_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber mapSub_; 
        ros::Publisher mapDilatePub_;
        
        boost::shared_ptr<MapBase> mapHandlerPtr_;
        boost::shared_ptr<PlannerBase> globalPathPlannerPtr_; 
        vector<geometry_msgs::PoseStamped> poses_;   
};

TestPathGen::TestPathGen(){
    mapHandlerPtr_ = boost::make_shared<MapBase>();
    globalPathPlannerPtr_ = boost::make_shared<AstarPlanner>(mapHandlerPtr_);
    // globalPathPlannerPtr_ = boost::make_shared<JPSPlanner>(mapHandlerPtr_);

    // ros 
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &TestPathGen::RvizPosesCallback, this);
    mapSub_ = nh_.subscribe("/map", 10, &TestPathGen::MapCallback, this);
    mapDilatePub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/map_dilate", 1, true);
}


// 地图回调函数
void TestPathGen::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    GLOG_INFO << "[MapCallback] Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;
    mapHandlerPtr_->setOccupancyGrid(msg);

    // 简单的地图膨胀 (膨胀半径为0.25)
    mapHandlerPtr_->SimpleDilateOccupancyGrid(0.25);

    // 发布膨胀后的地图
    auto map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>();
    if(mapHandlerPtr_->getMap(map_ptr, msg->header.frame_id)){
        mapDilatePub_.publish(*map_ptr);
    }
}


// Rviz位姿回调函数
void TestPathGen::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    GLOG_INFO << "[RvizPosesCallback] Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation);
    int poses_size = poses_.size();
    switch(poses_.size()){
    case 0:
        poses_.emplace_back(*msg);
        GLOG_INFO << "[RvizPosesCallback] set start point. poses_size: " 
            << poses_.size();
        break;
    case 1:
        poses_.emplace_back(*msg);
        GLOG_INFO << "[RvizPosesCallback] set goal point. poses_size: " 
            << poses_.size();
        // break;
    case 2:
        GLOG_INFO << "[RvizPosesCallback] planning start... poses_size: " 
            << poses_.size();
        {
            // 开始规划路径
            vector<geometry_msgs::Pose> poses;
            globalPathPlannerPtr_->pathPlanning(poses_.front().pose, poses_.back().pose, poses);
        }
        // 清空数据
        poses_.clear();
        GLOG_INFO << "[RvizPosesCallback] clear. poses_size: " << poses_.size();
        break;
    default:
        // 清空数据
        poses_.clear();
        GLOG_INFO << "[RvizPosesCallback] clear. poses_size: " << poses_.size();
        break;
    }
}


int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::INFO;       // 只记录大于{}等级的日志
    // FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    // 捕获系统信号
    google::InstallFailureSignalHandler();

    ros::init(argc, argv, "path_gen_test");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("path_gen_test start...");

    TestPathGen path_gen_tes;
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}